#ifndef _OBSTACLE_GRID_H_
#define _OBSTACLE_GRID_H_

#include <iostream>
#include <string.h>
#include <cmath>

#include <ros/ros.h>
#include <ros/console.h>
#include <ros/time.h>
#include <ros/duration.h>
#include <sensor_msgs/LaserScan.h>

// #include <windows.h>

using namespace std;

#define Diff_wheelSpacing 0.6
#define INFLATION_RADIUS (Diff_wheelSpacing/2+0.6)  //0.9
#define GRID_DEGREE 0.2 // 0.2 degree
#define GRID_ANGLE_LEN ((int)(180 / GRID_DEGREE))
#define IDX_TO_DEGREE(x) ((x) * GRID_DEGREE - 90)
#define DEGREE_TO_IDX(x) (((x) + 90) / GRID_DEGREE)

#define GRID_DIS 4 // 4cm
#define GRID_DIS_MAX 300
#define GRID_NUM (GRID_DIS_MAX / GRID_DIS) // 75

class ObstacleGrid
{
public:
    ObstacleGrid();
    ~ObstacleGrid();

    void process_data(const sensor_msgs::LaserScan::ConstPtr &scan);

    bool detect_obstacle(float width, float height);

    float find_anlge_of_way(float target_angle, float target_distance, float safe_distance);

private:
    void process_data_angle(const sensor_msgs::LaserScan::ConstPtr &scan);
    void process_data_grid(const sensor_msgs::LaserScan::ConstPtr &scan);
    void clear_obstacle();

    uint8_t deadzone_turn_flag = 0; //1 left, 2 right
    bool bdebug;
    int inflation_size = 0.8 * 100 / GRID_DIS;

public:
    // data
    int32_t obstacle_raw[GRID_NUM][GRID_NUM];       // 4cm 
    int32_t obstacle_inflation[GRID_NUM][GRID_NUM]; // 4cm 
    //                  0 (90) idx = 90/0.2 = 450
    //                  |
    //    (180)90-------|----------  -90 (0)
    float obs_angle_inflation[GRID_ANGLE_LEN]; // 180/0.2 = 900
};

#endif
